#include <ros/ros.h>
#include <irb_msgs/ObjectPose.h>
#include <tf/tf.h>

#include <opencv-2.3.1/opencv2/highgui/highgui.hpp>

#include <string>
#include <sstream>
using namespace std;

// GUI STUFF
string windowname;
int objectID = -1;

ros::Publisher posePub;

irb_msgs::ObjectPose pose1, pose2;

void drawCallback(const ros::TimerEvent&)
{
	double xdiff = pose2.pose.position.x - pose1.pose.position.x;
	double ydiff = pose2.pose.position.y - pose1.pose.position.y;
	double zdiff = pose2.pose.position.z - pose1.pose.position.z;

	double roll = atan2(ydiff, xdiff);
	double pitch = 0;
	double yaw = atan2(zdiff, xdiff);

	tf::Quaternion orientation = tf::createQuaternionFromRPY(roll, pitch, yaw);

	irb_msgs::ObjectPose msg;
	msg.ID = objectID;
	msg.pose.position.x = pose1.pose.position.x + xdiff/2.0;
	msg.pose.position.y = pose1.pose.position.y + ydiff/2.0;
	msg.pose.position.z = pose1.pose.position.z + zdiff/2.0;
	msg.pose.orientation.x = orientation.x();
	msg.pose.orientation.y = orientation.y();
	msg.pose.orientation.z = orientation.z();
	msg.pose.orientation.w = orientation.w();

	posePub.publish(msg);

	// Visualize object?

//	cv::imshow(windowname, image);
	cv::waitKey(1);
}

void firstObjCallback(const irb_msgs::ObjectPose &msg)
{
	pose1 = msg;
}

void secondObjCallback(const irb_msgs::ObjectPose &msg)
{
	pose2 = msg;
}

int main(int argc, char ** argv)
{
	ros::init(argc, argv, "simpleDetector");

	if(argc != 2)
	{
		ROS_ERROR("Missing or too many arguments, usage: %s <objectID>", argv[0]);
		return 1;
	}

	// Handle args
	for(int i = 1; i < argc; i++)
	{
		string arg(argv[i]);
		stringstream ss(arg);
		ss >> objectID;
		windowname = "SimpleTracker, ID: " + arg;
	}

	// GUI
	cv::namedWindow(windowname);

	ros::NodeHandle n;

	ros::Timer timer = n.createTimer(ros::Duration(0.1), drawCallback);

	ros::Subscriber sub1 = n.subscribe("input1", 100, firstObjCallback);
	ros::Subscriber sub2 = n.subscribe("input2", 100, secondObjCallback);

	posePub = n.advertise<irb_msgs::ObjectPose>("pose",100);

	ros::spin();

	return 0;
}
